COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Interpreter Initialization routines }
C00032 00003	{ Externally defined routines: }
C00035 00004	(* Initialization and wrapup routines: initArms & killArms *)
C00041 00005	(* Initialization routines: consDef, initWorld *)
C00047 ENDMK
C⊗;
{$NOMAIN	Interpreter Initialization routines }

const
	YARMDEV   = 1;		(* device numbers *)
	YHANDDEV  = 2;
	BARMDEV   = 3;
	BHANDDEV  = 4;
	VISEDEV   = 5;
	DRIVERDEV = 6;
	GARMDEV   = 7;
	GHANDDEV  = 8;
	RARMDEV   = 9;
	RHANDDEV  = 10;

	barmpower = 1B;		(* bit defs - used in response to initarmscmd *)
	yarmpower = 2B;
	garmpower = 4B;
	rarmpower = 10B;
	garmcal = 400B;
	rarmcal = 1000B;

	(* Constants from EDIT *)
      	maxLines = 28;		(* smaller on the 11 than on the 10 *)
      	maxPPLines = 18;
      	maxBpts = 25;
      	maxTBpts = 20;	(* max could be exceeded by huge case stmnt *)
	listinglength = 2000;	(* Length of Listingarray *)




(* Random type declarations for OMSI/SAIL compatibility *)

type

  byte = 0..255;	(* doesn't really belong here, but... *)
  ascii = char; 
  atext = text;

{ Define all the pointer types here }

vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
varidefp = ↑varidef;
nodep = ↑node;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
messagep = ↑message;
cmoncbp = ↑cmoncb;

dump = ↑integer;
token = array[1..4] of integer;	{Same size as a token}
cursorp = array[1..4] of integer;


(* datatype definitions *)

datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
	     frametype, eventtype, strngtype, labeltype, proctype, arraytype,
	     reftype, valtype, cmontype, nulltype, undeftype,
	     dimensiontype, mactype, macargtype, freevartype);

scalar = real;

u = (used,free);
vector = record case u of
	   used: (refcnt: integer; val: array [1..3] of real);
	   free: (next: vectorp);
	 end;

trans = record case u of
	   used: (refcnt: integer; val: array [1..3,1..4] of real);
	   free: (next: transp);
	end;

cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
linestr = packed array [1..130] of ascii;

strng = record
	  next: strngp;
	  ch: cstring;
	end;


event = record
	  next: eventp;		(* all events are on one big list *)
	  count: integer;
	  waitlist: pdbp;
	end;


frame = record
	  vari: varidefp;	(* back pointer to variable name & info *)
	  calcs: nodep;		(* affixment info *)
	  case ftype: boolean of	(* frame = true, device = false *)
  true:	    (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
  false:    (mech: integer; case sdev: boolean of
		true: (sdest: real); false: (tdest,appr,depr: transp));
		(* sdev = true for scalar devices, false for frames *)
	end;




(* statement definitions *)

stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
		fortype, iftype, whiletype, untiltype, casetype,
		calltype, returntype,
		printtype, prompttype, pausetype, aborttype, assigntype,
		signaltype, waittype, enabletype, disabletype, cmtype,
		affixtype, unfixtype,
		movetype, operatetype, opentype, closetype, centertype,
		stoptype, retrytype,
		requiretype, definetype, macrotype, commenttype, dimdeftype,
		setbasetype, wristtype, tovaltype, declaretype, emptytype);
		(* more??? *)


(* auxiliary definitions: variable, etc. *)

varidef = packed record
	    next,dnext: varidefp;
	    name: dump;
	    level: 0..255;	(* environment level *)
	    offset: 0..255;	(* environment offset *)
	    dtype: varidefp;	(* to hold the dimension info *)
	    tbits: 0..15;  (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
	    dbits: 0..15;	(* for use by debugger/interpreter *)
	    case vtype: datatypes of
  arraytype:  (a: nodep);
  proctype:   (p: nodep);
  labeltype,
  cmontype:   (s: dump);
  mactype:    (mdef: dump);
  macargtype: (marg: dump);
  pconstype:  (c: nodep);
  dimensiontype: (dim: nodep);
	  end;




(* definition of the ubiquitous NODE record *)

nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
		deprnode, viaptnode, apprnode, destnode, durnode,
		sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
		arrivalnode, departingnode,
		ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
		calcnode, arraydefnode, bnddefnode, bndvalnode,
		waitlistnode, procdefnode, tlistnode, dimnode, commentnode);

exprtypes =  (	svalop,					(* scalar operators *)
		sltop, sleop, seqop, sgeop, sgtop, sneop,	(* relations *)
		notop, orop, xorop, andop, eqvop,		(* logical *)
		saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
		sexpop, maxop, minop, intop, idivop, modop,
		sqrtop, logop, expop, timeop,			(* functions *)
		sinop, cosop, tanop, asinop, acosop, atan2op,	(* trig *)
		vdotop, vmagnop, tmagnop,
		vecop,					(* vector operators *)
		vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
		svmulop, vsmulop, vsdivop, tvmulop, wrtop,
		tposop, taxisop,
		transop,				(* trans operators *)
		tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
		vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
		ioop,					(* i/o operators *)
		queryop, inscalarop,
		specop,					(* special operators *)
		arefop, callop, grinchop, macroop, vmop, adcop, dacop,
		badop,
		addop, subop, negop, mulop, divop, absop); (* for parsing *)

leaftypes = pconstype..strngtype;

reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);

node = record
	next: nodep;
	case ntype: nodetypes of
    exprnode:	(op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
    leafnode:	(case ltype: leaftypes of
	varitype:  (vari: varidefp; vid: dump);
	pconstype: (cname: varidefp; pcval: nodep);
	svaltype:  (s: scalar; wid: integer);
	vectype:   (v: vectorp);
	transtype: (t: transp);
	strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
    listnode:	(lval: nodep);
	end;





(* process descriptor blocks & environment record definitions *)

queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
		forcewait,devicewait,joinwait,proccall);

pdb = packed record
	nextpdb,next: pdbp;	(* for list of all/active pdb's *)
	level: 0..255;		(* lexical level *)
	mode: 0..255;		(* expression/statement/sub-statement *)
	priority: 0..255;
	status: queuetypes;	(* what are we doing *)
	env: envheaderp;
	spc: dump;		(* current statement *)
	epc: nodep;		(* current expression (if any) *)
	sp: nodep;		(* intermediate value stack *)
	cm: cmoncbp;		(* if we're a cmon point to our definition *)
	mech: framep;		(* current device being used *)
	linenum: integer;	(* used by editor/debugger *)
	 case procp: boolean of	(* true if we're a procedure *)
true:  (opdb: pdbp;		(* pdb to restore when procedure exits *)
	pdef: nodep);		(* procedure definition node *)
false: (evt: eventp;		(* event to signal when process goes away *)
	sdef: dump);	(* first statement where process was defined *)
      end;


envheader = packed record
	      parent: envheaderp;
	      env: array [0..4] of environp;
	      varcnt: 0..255;		(* # of variables in use ??? *)
		case procp: boolean of  (* true if we're a procedure *)
	true: (proc: nodep);
	false:(block: dump);
	    end;


enventry = record
	    case etype: datatypes of
  svaltype:  (s: scalar);
  vectype:   (v: vectorp);
  transtype: (t: transp);
  frametype: (f: framep);
  eventtype: (evt: eventp);
  strngtype: (length: integer; str: strngp);
  cmontype:  (c: cmoncbp);
  proctype:  (p: nodep; penv: envheaderp);
  reftype:   (r: enventryp);
  arraytype: (a: envheaderp; bnds: nodep);
	   end;


environment = record
		next: environp;
		vals: array [0..9] of enventryp;
	      end;


cmoncb = record
	   running, enabled: boolean;		(* cmon's status *)
	   cmon: dump;
	   pdb: pdbp;
	   evt: eventp;
	   fbits: integer;			(* bits for force sensing *)
	   oldcmon: cmoncbp;			(* for debugger *)
	 end;


(* definition of AL-ARM messages *)

msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
	    abortcmd,stopcmd,movehdrcmd,movesegcmd,
	    centercmd,operatecmd,movedonecmd,signalcmd,readjtcmd,drivecmd,
	    setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
	    zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
	    errorcmd,floatcmd);

errortypes = (noerror,noarmsol,timerr,durerr,toolong,useopr,nosuchdev,featna,
	      unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
	      baddev,timout,panicb);

message = record
	   cmd: msgtypes;
	   ok: boolean;
	   case integer of
	1:   (dev, bits, n: integer;
(*	     (dev, bits, n, evt: integer;	(* for arm code version *)
	      evt: eventp;
	      dur: real;
	      case integer of
		1: (v1,v2,v3: real);
		2: (sfac,wobble,pos: real);
		3: (val,angle,mag: real);
		4: (max,min: real);
		5: (error: errortypes));
	2:   (fv1,fv2,fv3,mv1,mv2,mv3: real);	(* may never use these... *)
	3:   (t: array [1..6] of real);
	  end;

interr = record
         case integer of
           0: (i: integer);
	   1: (err,foo: errortypes);
	 end;



(* Global variables *)

var 
	(* From ALMAIN *)
    b:boolean;		
    ch:ascii;
    ltime: real;

	(* From PARSE *)
    reswords: array [0..26] of dump;
    idents: array [0..26] of dump;
    macrostack: array [1..10] of dump;
    curmacstack: array [1..10] of varidefp;
    macrodepth: integer;
    curchar, maxchar, curline: integer;
    curBlock,newDeclarations: dump;
    curProc: varidefp;
    pnode: nodep;
    nodim, distancedim, timedim, angledim,
      forcedim, torquedim, veldim, angveldim: varidefp;
    fvstiffdim, mvstiffdim: nodep;
    filedepth: integer;
    curpage: integer;
    sysVars,unVars: varidefp;
    errcount: integer;
    outerBlock: dump;
    curVariable: varidefp;
    curMotion: dump;
    endOk,coendOk: integer;
    moveLevel: integer;
    curErrhandler, curCmon: dump;
    endOfLine, backup, expandmacros, flushcomments, dimCheck: boolean;
    semiseen, shownline: boolean;
    eofError: boolean;
    inMove,inCoblock: boolean;
    curtoken: token;	
    file1,file2,file3,file4,file5: atext;
    line: linestr;

	(* From INTERP *)
    curInt, activeInts, readQueue, allPdbs: pdbp;
    curEnv, sysEnv: envheaderp;
    clkQueue: nodep;
    allEvents: eventp;
    STLevel: integer;		(* set by GO *)
    etime: integer;		(* used by eval *)
    curtime: integer; 		(* Time of day, in ticks *)
    stime: integer;		(* used for clock queue on 10 *)
    msg: messagep;		(* for AL-ARM interaction *)
    inputp: integer;		(* current offset into inputLine array above *)
    debugLevel: integer;
    tSingleThreadMode: boolean;
    resched, running, escapeI, singleThreadMode: boolean;
    msgp: boolean;		(* flag set if any messages pending *)
    inputReady: boolean;
    inputLine: array [1..20] of ascii;

	(* From EDIT *)
    lines: array [1..maxLines] of dump; 
    ppLines: array [1..maxPPLines] of dump;	
    marks: array [1..20] of integer;
    cursorStack: array [1..15] of cursorp;
    bpts: array [1..maxBpts] of dump;
    tbpts: array [1..maxTBpts] of dump;
    debugPdbs: array [0..10] of pdbp;
    screenheight,dispHeight: integer;
    ppBufp,oppBufp,ppOffset,ppSize,nmarks: integer;
    lbufp,cursor,ocur,cursorLine,fieldnum,lineNum,findLine,pcLine: integer;
    firstDline,topDline,botDline,firstLine,lastLine: integer;
    freeLines,oldLines: dump;
    findStmnt: dump;
    nbpts,ntbpts: integer;
    eCurInt: pdbp;
    dProg: dump;
    smartTerminal: boolean; 
    setUp,setExpr,setCursor,dontPrint,outFilep,newVarOk,collect: boolean;
    eBackup: boolean;			
    eSingleThreadMode: boolean;	
    listing: packed array [0..listinglength] of ascii;
    lbuf: array [1..160] of ascii;
    ppBuf: array [1..100] of ascii;
    outFile: atext;
    eCurToken: token;	

	(* Various device & variable pointers *)
    speedfactor: enventryp;
    barm: framep;

	(* Various constant pointers *)
    xhat,yhat,zhat,nilvect: vectorp;
    niltrans: transp;
    bpark, ypark, gpark, rpark: transp;		(* arm park positions *)

{ Externally defined routines: }

	(* From ALLOC *)
function newEheader: envheaderp; 				external;
function newEnvironment: environp;				external;

	(* From IAUX1A *)
function getPdb: pdbp;						external;
function getEntry (level, offset: byte): enventryp; 		external;
procedure makeVar(e: enventryp; vari: varidefp; tbits: integer);external;
function enterEntry (var i,j: integer; var env: environp;
		 envhdr: envheaderp; v: varidefp): enventryp;	external;
procedure sendCmd;						external;
procedure sendTrans(tr: transp);				external;

	(* From IAUX2A *)
procedure getReply;						external;
function whereArm (mech: integer): transp;			external;

	(* Arithmetic Routines *)
function vmake (a,b,c: scalar): vectorp; 			external;
function tmake (t: transp; v: vectorp): transp; 		external;
function vsaxwr(ax: vectorp; w: real): transp; 			external;

	(* From RSXRUT *)
procedure initMsg(var buf: messagep; var flag: boolean);	external;

	(* From RSXMSG *)
function startArm: boolean;                                  	external;

	(* From PP *)
procedure ppLine; 						external;
procedure ppOutNow; 						external;
procedure ppChar(ch: ascii); 					external;
procedure pp5(ch: c5str; length: integer); 			external;
procedure pp10(ch: cstring; length: integer); 			external;
procedure pp10L(ch: cstring; length: integer);			external;
procedure pp20(ch: c20str; length: integer); 			external;
procedure pp20L(ch: c20str; length: integer); 			external;

	(* From DISP *)
function getChar: ascii; 					external;
procedure escInit(var flg: boolean); 				external;

(* Initialization and wrapup routines: initArms & killArms *)

procedure initArms; external;
procedure initArms;
 var b: boolean; i: integer; ch: ascii; 

 function bitOn(i: integer): boolean;
  begin bitOn := (msg↑.bits AND i) <> 0 end;	(* *** non-standard Pascal *** *)

 function getPromptChar: ascii;
  var ch,dch: ascii;
  begin
  ch := getChar;			(* Read one character *)
  if ord(ch)=15B then ch := ' ';	(* Convert CR to space *)
  ppChar(ch);				(* and echo it *)
  if ('a' <= ch) and (ch <= 'z') then ch := chr(ord(ch)-40B);	(* To upper case *)
  getPromptChar := ch;
  end;

 procedure whichArm;
  begin
  case i of	(* tell which arm *)
1: pp5('YARM ',4);
2: pp5('BARM ',4);
3: pp5('GARM ',4);
4: pp5('RARM ',4);
   end;
  end;

 begin
 initMsg(msg,msgp);		(* connect to message buffer *)
 msgp := false;
(* b := startArm;			(* get ARM servo running *)
(* *** *) b := true;		(* Someday this will work... *)
 if not b then begin		(* Complain if error during startup *)
(* Probably should set some global flag so we don't try to talk to ARM *)
(* or maybe even exit the program *)
   pp20L('Error during ARM sta',20); pp20('rtup!  Arms not init',20); 
   pp10('ialized.  ',8); ppLine end
  else 
   begin
   for i := 3 to 4 do		(* try to init just the PUMAs for now *)
    begin
    repeat
     with msg↑ do
      begin
      ch := ' ';
      cmd := initarmscmd;
      case i of
  1:   dev := yarmdev;
  2:   dev := barmdev;
  3:   dev := garmdev;
  4:   dev := rarmdev;
       end;
      getReply;			(* send over init command & wait for reply *)
      case i of
  1:   b := bitOn(yarmpower);
  2:   b := bitOn(barmpower);
  3:   b := bitOn(garmpower);
  4:   b := bitOn(rarmpower);
       end;
      b := ok and b;
      if not ok then
	pp20L('Couldn''t initialize ',20)
       else if not b then
	pp20L('Power off for       ',14);
      if not b then begin whichArm; ppOutNow end;

      if ok and (i >= 3) then		(* try to calibrate PUMA's *)
	begin
	while not b do			(* get power turned on *)
	 begin
         pp20L('Turn on arm high pow',20); 
	 pp20 ('er (Type SPACE to co',20);
	 pp20 ('ntinue, any other to',20);
	 pp10 (' abort):  ',9);
	 ppOutNow;
         ch := getPromptChar;
	 if ch <> ' ' then	(* any letter will abort *)
	   begin
	   pp10L(' Aborted  ',8);
	   if not(((i=3) and bitOn(garmcal)) or
		  ((i=4) and bitOn(rarmcal))) then
	     begin pp20(' - arm not calibrate',20); pp5('d!   ',2); ppLine end
	    else ppLine;
	   ppOutNow;
	   b := true;		(* so we leave power up loop *)
	   end
	  else
	   begin			(* keep trying *)
	   getReply;		(* retry the init command & check power *)
	   if i = 3 then b := bitOn(garmpower)
	    else b := bitOn(rarmpower);
	   b := b and ok;
	   end;
	 end;
	if ch <> ' ' then b := false
	 else if i = 3 then b := bitOn(garmcal)
	  else b := bitOn(rarmcal);
	if (ch = ' ') and not b then	(* if not already calibrated ... *)
	  begin
	  pp20L('Type Y to calibrate ',20); whichArm;
	  ppOutNow;
	  ch := getPromptChar;
	  if (ch = 'Y') then
	    begin
	    cmd := calibcmd;
	    getReply;			(* go calibrate arm *)
	    if ok then pp20L('Calibration complete',20)
	     else begin pp20L('Error while calibrat',20); pp5('ing  ',3); end;
	    b := ok;
	    end
	   else begin pp20L(' Aborted - arm not c',20); pp10('alibrated ',9); end;
	  ppLine; ppOutNow;
	  end;
	end;
      end;
     if not b then
      begin
      pp20L('Type Y to try again:',20); ppchar(' '); ppOutNow;
      ch := getPromptChar;
      b := (ch <> 'Y');
      end
    until b;
    end;
   end;
 end;


procedure killarms; external;
procedure killarms;
 begin
 msg↑.cmd := killarmscmd;
 sendCmd;
 end;

(* Initialization routines: consDef, initWorld *)

{ consDef is one of the first initialization routines called, so let's	}
{ tell what we're doing while we're in here.				}

procedure consDef; external;
procedure consDef;
 var i: integer;
 begin
 for i:=1 to 15 do writeln;
 writeln('                            Welcome to AL Version 2.A');
 for i:=1 to 3 do writeln;
 writeln('                                  Initializing...');
 for i:=1 to 8 do writeln;
 xhat := vmake(1,0,0); xhat↑.refcnt := 1000;
 yhat := vmake(0,1,0); yhat↑.refcnt := 1000;
 zhat := vmake(0,0,1); zhat↑.refcnt := 1000;
 nilvect := vmake(0,0,0); nilvect↑.refcnt := 1000;
 niltrans := tmake(vsaxwr(zhat,0.0),nilvect); niltrans↑.refcnt := 1000;
 ypark := tmake(vsaxwr(yhat,180.0),vmake(43.5,2.325,6.86));
 bpark := tmake(vsaxwr(yhat,180.0),vmake(43.53125,56.855,9.95875));
 gpark := tmake(vsaxwr(zhat,180.0),vmake(83.2,46.13,67.7));
 rpark := tmake(niltrans,vmake(84.8,12.87,67.7));
 ypark↑.refcnt := 1000;
 bpark↑.refcnt := 1000;
 gpark↑.refcnt := 1000;
 rpark↑.refcnt := 1000;
 end;

procedure initWorld; external;
procedure initWorld;
 var v: varidefp; e: enventryp; i,j: integer; envir: environp;
     b: boolean;
 begin
 initArms;			(* *** should this go here ??? *** *)
 etime := 0;
 curtime := 0;
 activeInts := nil;		(* zero the various queues *)
 clkQueue := nil;
 readQueue := nil;
 allPdbs := nil;
 curInt := nil;
 allEvents := nil;
 resched := false;
 singleThreadMode := false;
 sysEnv := newEheader;		(* set up system variables *)
 with sysEnv↑ do
  begin
  parent := nil;
  block := nil;
  procp := false;
  envir := newEnvironment;
  env[0] := envir;
  for i := 1 to 4 do env[i] := nil;
  end;
 i := 0;
 j := -1;
 v := sysVars;		(* get list of predefined system variables *) (* Instead of "getsysvars" *)
 while v <> nil do
  begin
(* need to handle devices specially - especially scalar devices *)
  e := enterEntry(i,j,envir,sysEnv,v);
  b := v↑.offset in [0,2,4,6,8,10,12,14,16,20];
	(* offsets: arms: 0,4,8,12  hands: 2,6,10,14  driver/vise: 16,20 *)
  if b then e↑.etype := frametype; (* so we get a frame for scalar devices *)
  makeVar(e,v,v↑.tbits);		(* make variable environment entry *)
  if b then					(* set up device values *)
   with e↑.f↑ do
    begin
    ftype := false;				(* it's a device *)
    sdev := v↑.vtype = svaltype;		(* indicate if scalar *)
    if sdev then sdest := 0
     else
      begin
      tdest := niltrans;
      appr := nil;
      depr := nil;
      end;
    case v↑.offset div 2 of			(* set Mechanism bits *)
   0:	mech := barmdev;	(* barm *)
   1:	mech := bhanddev;	(* bhand *)
   2:	mech := yarmdev;	(* yarm *)
   3:	mech := yhanddev;	(* yhand *)
   4:	mech := garmdev;	(* garm *)
   5:	mech := ghanddev;	(* ghand *)
   6:	mech := rarmdev;	(* rarm *)
   7:	mech := rhanddev;	(* rhand *)
   8:	mech := driverdev;	(* driver *)
  10:	mech := visedev;	(* vise *)
  otherwise {do nothing - shouldn't happen.. };
     end;
    end;
  v := v↑.next
  end;
 for i := j+1 to 9 do envir↑.vals[i] := nil;
 speedfactor := getEntry(0,24);
 e := getEntry(0,0);			(* offset for barm = 0 *)
 barm := e↑.f;				(* remember frame used for blue arm *)
 curInt := getPdb;
 escInit(escapeI);			(* enable escape-I interrupts *)
 end;